#include <controller/control_node.hpp>

 mavros_msgs::State current_state;
 void state_cb(const mavros_msgs::State::ConstPtr& msg){
     current_state = *msg;
 }
 
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "offb_node");
     ros::NodeHandle nh;
 
     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
             ("mavros/state", 10, state_cb);
     ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
             ("mavros/setpoint_position/local", 10);

     //the setpoint publishing rate MUST be faster than 2Hz
     ros::Rate rate(20.0);
 
     // wait for FCU connection 非阻塞式回调，直到连接成功
     while(ros::ok() && !current_state.connected){
         ros::spinOnce();       //处理一次回调
         rate.sleep();          //控制频率在20Hz
     }
 
     geometry_msgs::PoseStamped pose;
     pose.pose.position.x = 0;
     pose.pose.position.y = 0;
     pose.pose.position.z = 0;
 
     //send a few setpoints before starting
     for(int i = 100; ros::ok() && i > 0; --i){
         local_pos_pub.publish(pose);
         ros::spinOnce();
         rate.sleep();
     }
 

 
     while(ros::ok()){
        if( current_state.mode != "OFFBOARD"){
            ROS_INFO("waitting for offboard");
        } 
         else {
            if( !current_state.armed){
                ROS_INFO("Vehicle armed");
            }
        } 
         ros::spinOnce();
         rate.sleep();
     }
 
     return 0;
 }